package pack.model;

import java.awt.Color;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.awt.Shape;
import java.awt.geom.AffineTransform;
import java.awt.geom.Line2D;
import java.awt.geom.Line2D.Double;
import java.awt.geom.Line2D.Float;
import java.awt.geom.Rectangle2D;
import java.util.ArrayList;

import pack.help.State;
import pack.help.Vec;
import pack.model.controllers.*;

public class Robot {
	private Vec pos;
	private float fi;
	// velocity of the RIGHT wheel
	private float v1;
	// velocity of the LEFT wheel
	private float v2;
	
	// the controller controlling the robot
	private Controller controller;
	// points which define the path taken so far
	private ArrayList<Vec> path = new ArrayList<Vec>();
	
	// color of the robot
	private final Color color;
	// name of the robot
	//private final String name;

	
	// moves the robot
	// t = time interval
	// v1 = right wheel velocity
	// v2 = left wheel velocity
	// target = target state
	public void move(int t, State target){
		// the controller's orders for delta v1 and v2;
		double[] deltaV = new double[2];
		float tS = (float) t / 1000.f;
		// we assume that if v1 and v2 accelerates with a1 and a2
		// for a small time, the movement of the robot equals
		// as the robot moved with (v1+a1*t/2) and (v2+a2*t/2) wheel velocities
		float oldV1 = v1;
		float oldV2 = v2;
		
		
		// Current state
		State current = new State(pos,fi,this.v1,this.v2);
		
		deltaV = controller.control(current, target,getDir(),tS);
		if (deltaV[0] >  RealLifeProperties.maxA*tS) deltaV[0] =  RealLifeProperties.maxA*tS;
		if (deltaV[1] >  RealLifeProperties.maxA*tS) deltaV[1] =  RealLifeProperties.maxA*tS;
		if (deltaV[0] < -RealLifeProperties.maxA*tS) deltaV[0] = -RealLifeProperties.maxA*tS;
		if (deltaV[1] < -RealLifeProperties.maxA*tS) deltaV[1] = -RealLifeProperties.maxA*tS;
		this.v1 += deltaV[0];
		this.v2 += deltaV[1];
		if (this.v1 >  RealLifeProperties.maxV) this.v1 =  RealLifeProperties.maxV;
		if (this.v2 >  RealLifeProperties.maxV) this.v2 =  RealLifeProperties.maxV;
		if (this.v1 < -RealLifeProperties.maxV) this.v1 = -RealLifeProperties.maxV;
		if (this.v2 < -RealLifeProperties.maxV) this.v2 = -RealLifeProperties.maxV;
		State outcome = new State(idealMove(t,(this.v1+oldV1)/2,(this.v2+oldV2)/2));
		//State outcome = new State(target);
		// overriding the parameters to reflect our new state
		stateToParams(outcome);
	}
	
	// a function which calculates the ideal trajectory and movement
	public State idealMove(int t, float v1, float v2){
		// radius in meters
		float r;
		// angular velocity
		float omega;
		// A vector pointing to the center of rotation
		Vec toO;
		// Rotation center
		Vec o;
		// Rotation in radians
		float alpha;
		// t in SEC
		float ts = ((float)t)/1000.f;
		// the function makes modofications on this "virtual" state
		State ret = new State(pos,fi,v1,v2);
		
		// If the trajectory is not linear
		if (Math.abs(v1-v2)>0.001){
			r = RealLifeProperties.l*(v2+v1)/(v2-v1);
			omega = (v2-v1)/(2*RealLifeProperties.l);
			// toO = [-sin(fi);cos(fi)]
			toO = new Vec(-1*((float)Math.sin(fi)),(float)Math.cos(fi));
			// o = ret +  r*toO
			o = new Vec(ret.pos.addNR(toO.multiply(r)));
			
			alpha = omega * ts;
			
			Vec tmp = ret.pos.add(o.multiplyNR(-1));
			// new position
			ret.pos = o.add(new Vec(tmp.getX()*(float)Math.cos(alpha)-tmp.getY()*(float)Math.sin(alpha),
							tmp.getX()*(float)Math.sin(alpha)+tmp.getY()*(float)Math.cos(alpha)));
			// new orientation
			ret.fi = rotate(ret.fi,alpha);
		} else {
			float v = v1;
			Vec i = new Vec(((float)Math.cos(fi)),(float)Math.sin(fi));
			ret.pos.add(i.multiplyNR(v*ts));
		}
		
		path.add(new Vec(ret.pos));
		return ret;
	}
	
	public float rotate(float fi, float angle){
		float ret = fi;
		ret+=angle;
		return ret;
		/*
		if (fi > Math.PI) {
			fi = (float) (2*Math.PI-fi);
		}
		if (fi < -Math.PI){
			fi += 2*Math.PI+fi;
		}*/
	}
	
	public void draw(Graphics g){
		g.setColor(color);
		Graphics2D g2d = (Graphics2D)g;
		// drawing the PATH
		Vec oldpoint = null;
		Float line = null;
		for (Vec newpoint: path){
			if (path.size() > 0){
				if (oldpoint != null){
					if (oldpoint.getX()>0 && oldpoint.getY()>0 &&newpoint.getX()>0 && newpoint.getY()>0){
						line =  new Line2D.Float(oldpoint.getX()*100,oldpoint.getY()*100,newpoint.getX()*100,newpoint.getY()*100);
						g2d.draw(line);
					}
					
				} 
				oldpoint = new Vec (newpoint);
			}
		}
		
		
		// drawing the BODY
		g.setColor(color);
		Rectangle2D body = new Rectangle2D.Double(((pos.getX()-RealLifeProperties.d)*100),((pos.getY()-RealLifeProperties.l)*100),(RealLifeProperties.d*2*100),(RealLifeProperties.l*2*100));
		AffineTransform trans = new AffineTransform();
		trans.rotate(fi, pos.getX()*100, pos.getY()*100);
		Shape rotatedBody = trans.createTransformedShape(body);
		
		g2d.draw(rotatedBody);
	//	g2d.fill(rotatedBody);
		
		// drawing the LINES
		g.setColor(Color.BLACK);
		Double line1 = new Line2D.Double((pos.getX()-RealLifeProperties.d)*100,(pos.getY()-RealLifeProperties.l)*100,(pos.getX()+RealLifeProperties.d)*100,(pos.getY())*100);
		Double line2 = new Line2D.Double((pos.getX()-RealLifeProperties.d)*100,(pos.getY()+RealLifeProperties.l)*100,(pos.getX()+RealLifeProperties.d)*100,(pos.getY())*100);
		g2d.draw(trans.createTransformedShape(line1));
		g2d.draw(trans.createTransformedShape(line2));
	}
	
	public Robot(Color c, String name, Controller cont){
		fi = (float) (Math.PI/2.0);
		//fi = 0.f;
		pos = new Vec(1.3f,0.2f);
		path.add(pos);
		v1 = v2 = 0;
		controller = cont;
		color = c;
	}
	
	public State getState(){
		return (new State(pos,fi,this.v1,this.v2));
	}
	
	public void stateToParams(State s){
		v1 = s.v1;
		v2 = s.v2;
		fi = s.fi;
		pos = new Vec(s.pos);
	}
	
	// return a unit vector pointing at the direction of the robot
	public Vec getDir(){
		return new Vec(Math.cos(fi),Math.sin(fi));
	}
	
	public Vec getPos() {
		return pos;		
	}
	public void setPos(Vec pos) {
		this.pos = pos;
	}
	public float getFi() {
		return fi;
	}
	public void setFi(float fi) {
		this.fi = fi;
	}
}
